/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * @file           : main.c
  * @brief          : Main program body
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2025 STMicroelectronics.
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the LICENSE file
  * in the root directory of this software component.
  * If no LICENSE file comes with this software, it is provided AS-IS.
  *
  ******************************************************************************
  */
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "can.h"
#include "gpio.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "CAN_receive.h"
#include "bsp_can.h"
#include "pid.h"
//#include "../../application/CAN_receive.h"
/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */
//接收数据结构体
  // pid_type_def       motor_pid;//PID数据结构体1
   extern PID_TypeDef drive_motor_pid[4];//PID数据结构体2

   motor_measure_t   *motor_data[4];  //电机结构体指针

   //fp32 PID[3]={10,0.02,0};
/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */

/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/

/* USER CODE BEGIN PV */
// FreeMaster可访问的全局变量
//速度环控制
// 在main.c中修改速度环参数
float target_speed = 500.0f;       // 先从小速度开始
float motor_kp = 8.0f;             // 大幅减小KP
float motor_ki = 0.5f;             // 适当KI
float motor_kd = 0.1f;             // 大幅减小KD
float actual_speed[4] = {0};        // 存储实际速度
float pid_output[4] = {0};          // 存储PID输出值

//位置环控制
float target_angle =0.0f;        // 目标角度（度）
float actual_angle[4] = {0};        // 实际角度（度）
float position_kp = 200.0f;           // 位置环P参数
float position_ki = 0.5f;          // 位置环I参数
float position_kd = 2000.0f;           // 位置环D参数
uint8_t control_mode = 1;           // 0:速度环, 1:位置环
// 添加调试变量
float angle_error[4] = {0};         // 角度误差
float normalized_target = 0;        // 规范化后的目标角度
float normalized_actual = 0;        // 规范化后的实际角度

/* 添加圈数计数变量 */
float motor_rounds[4] = {0};        // 存储电机圈数
float motor_total_angle[4] = {0};   // 存储电机总角度

float position_output_target_speed[4] = {0};
/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
/* USER CODE BEGIN PFP */

/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */
// 强制位置环输出测试
/*void Force_Position_Output(void)
{
  static uint32_t last_force_time = 0;
  static int16_t force_output = 0;

  if (HAL_GetTick() - last_force_time > 2000) {
    // 强制输出一个较大的电流值
    force_output = (force_output == 0) ? 8000 : 0;
    CAN_cmd_chassis(force_output, 0, 0, 0);
    // printf("FORCE OUTPUT: %d\n", force_output);
    last_force_time = HAL_GetTick();
  }
}*/

/*void Debug_Position_Control(void)
{
  static uint32_t last_debug_time = 0;
  if (HAL_GetTick() - last_debug_time > 500) {
    // 打印调试信息
    // printf("Target: %.1f, Actual: %.1f, Error: %.1f, Output: %.1f\n",
    //        target_angle, actual_angle[0],
    //        target_angle - actual_angle[0], pid_output[0]);
    last_debug_time = HAL_GetTick();
  }
}*/

// 在USER CODE BEGIN 0部分添加测试函数
/*void Test_Reverse_Rotation(void)
{
  static uint32_t last_test_time = 0;
  static uint8_t test_phase = 0;
  static float test_angles[] = {0, 180, 90, 270, 45}; // 测试角度序列

  if (HAL_GetTick() - last_test_time > 3000) { // 每3秒切换一次目标
    target_angle = test_angles[test_phase];
    test_phase = (test_phase + 1) % 5;
    last_test_time = HAL_GetTick();

    // 强制输出一个负值测试反向转动
    if (test_phase == 1) { // 当切换到180度时
      CAN_cmd_chassis(-3000, 0, 0, 0); // 强制反向输出
      HAL_Delay(100); // 短暂输出
    }
  }
}*/
/* USER CODE END 0 */

/**
  * @brief  The application entry point.
  * @retval int
  */
int main(void)
{

  /* USER CODE BEGIN 1 */

  /* USER CODE END 1 */

  /* MCU Configuration--------------------------------------------------------*/

  /* Reset of all peripherals, Initializes the Flash interface and the systick. */
  HAL_Init();

  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* Configure the system clock */
  SystemClock_Config();

  /* USER CODE BEGIN SysInit */

  /* USER CODE END SysInit */

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_CAN1_Init();
  /* USER CODE BEGIN 2 */
can_filter_init();
  //CAN_cmd_chassis(-100,0,0,0);
  //HAL_Delay( 200);
  //CAN_cmd_chassis(1000,1000,100,100);

  // 修改速度环初始化参数
  /*for (int di = 0; di < 4; di++) {
    pid_init(&drive_motor_pid[di]);
    drive_motor_pid[di].f_param_init(&drive_motor_pid[di], PID_Speed,
                                     10000,    // 减小最大输出
                                     5000,     // 积分限制
                                     10.0f,     // 死区
                                     2,
                                     500,      // 最大误差
                                     0,
                                     8.0f,     // 保守的KP
                                     0.5f,     // KI
                                     0.1f);    // 很小的KD
  }*/

  // 初始化位置环PID
  for (int di = 0; di < 4; di++) {
    pid_init(&drive_motor_pid[di]);
    // 位置环参数
    drive_motor_pid[di].f_param_init(&drive_motor_pid[di], PID_Position,     // 增加最大输出到10000
                                     5000,     // 最大输出
                                     1000,     // 积分限制
                                     3,       // 死区（编码器值）
                                     2,
                                     32,     // 最大误差512（45度）
                                     0,        // 初始目标值
                                     2.0f,     // KP
                                     0.05f,    // KI
                                     0.1f);    // KD
  }
  //捕获各电机的状态值
  motor_data[0] = get_chassis_motor_measure_point( 0);//获取ID为1号的电机数据指针
  motor_data[1] = get_chassis_motor_measure_point( 1);//获取ID为2号的电机数据指针
  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1) {
static uint32_t last_debug_time_led1 = 0;
  if (HAL_GetTick() - last_debug_time_led1 > 500) {
    HAL_GPIO_TogglePin(LED1_GPIO_Port, LED1_Pin);
    last_debug_time_led1 = HAL_GetTick();
  }
//
static uint32_t last_debug_time_led2 = 0;
 if (HAL_GetTick() - last_debug_time_led2 > 500) {
    HAL_GPIO_TogglePin(LED2_GPIO_Port, LED2_Pin);
    last_debug_time_led2 = HAL_GetTick();
  }
    // 更新圈数和角度数据
    for (int i = 0; i < 4; i++) {
      if (motor_data[i] != NULL) {
        motor_rounds[i] = motor_data[i]->round_count;
        motor_total_angle[i] = motor_data[i]->total_angle;
      }
    }
    if (control_mode == 0) {
      // ==================== 速度环控制 ====================
      for (int i = 0; i < 4; i++) {
        if (motor_data[i] != NULL) {
          drive_motor_pid[i].target = target_speed;
          drive_motor_pid[i].kp = motor_kp;
          drive_motor_pid[i].ki = motor_ki;
          drive_motor_pid[i].kd = motor_kd;
          drive_motor_pid[i].f_cal_pid(&drive_motor_pid[i], motor_data[i]->speed_rpm);
          actual_speed[i] = motor_data[i]->speed_rpm;
          pid_output[i] = drive_motor_pid[i].output;  // ← 保留这一行

          // 限制输出范围
          if (pid_output[i] > 16384) pid_output[i] = 16384;
          //if (pid_output[i] <100 &&pid_output[i] > 50) pid_output[i] = 100;
          //if (pid_output[i] <-50 &&pid_output[i] > -100) pid_output[i] = -100;
          if (pid_output[i] < -16384) pid_output[i] = -16384;
        }
      }
    } else {
       // ==================== 位置环控制（两层PID） ====================
        for (int i = 0; i < 4; i++) {
            if (motor_data[i] != NULL) {
                // 获取当前位置和速度
                float current_angle = motor_data[i]->total_angle;
                float current_speed = motor_data[i]->speed_rpm;

                // 第一层：位置环PID计算目标速度
                // 设置位置环PID参数
                drive_motor_pid[i].target = target_angle;
                drive_motor_pid[i].kp = position_kp;
                drive_motor_pid[i].ki = position_ki;
                drive_motor_pid[i].kd = position_kd;

                // 计算位置环PID输出（这个输出作为速度环的目标速度）
                drive_motor_pid[i].f_cal_pid(&drive_motor_pid[i], current_angle);
                float target_speed_from_position = drive_motor_pid[i].output;

                // 限制位置环输出的目标速度范围
                float max_target_speed = 2000.0f;  // 最大目标速度限制
                if (target_speed_from_position > max_target_speed)
                    target_speed_from_position = max_target_speed;
                if (target_speed_from_position < -max_target_speed)
                    target_speed_from_position = -max_target_speed;

              // 保存位置环计算的目标速度（用于FreeMaster调试）
              position_output_target_speed[i] = target_speed_from_position;
                // 第二层：速度环PID计算输出电流
                // 使用速度环PID参数
                drive_motor_pid[i].target = target_speed_from_position;  // 位置环的输出作为速度环的目标
                drive_motor_pid[i].kp = motor_kp;    // 速度环P参数
                drive_motor_pid[i].ki = motor_ki;    // 速度环I参数
                drive_motor_pid[i].kd = motor_kd;    // 速度环D参数

                // 计算速度环PID输出（最终输出电流）
                drive_motor_pid[i].f_cal_pid(&drive_motor_pid[i], current_speed);

                // 更新显示值和输出
                actual_angle[i] = current_angle;
              actual_speed[i] = current_speed;  // 更新实际速度显示
                angle_error[i] = target_angle - current_angle;
                pid_output[i] = drive_motor_pid[i].output;

                // 限制最终输出范围
                if (pid_output[i] > 10000) pid_output[i] = 10000;
                if (pid_output[i] < -10000) pid_output[i] = -10000;
            }
        }
    }


        // 发送控制命令 - 统一使用pid_output数组
        CAN_cmd_chassis((int16_t)pid_output[0],0,0,0);

        HAL_Delay(2);
    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */
  }
  /* USER CODE END 3 */
}

/**
  * @brief System Clock Configuration
  * @retval None
  */
void SystemClock_Config(void)
{
  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

  /** Configure the main internal regulator output voltage
  */
  __HAL_RCC_PWR_CLK_ENABLE();
  __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1);

  /** Initializes the RCC Oscillators according to the specified parameters
  * in the RCC_OscInitTypeDef structure.
  */
  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
  RCC_OscInitStruct.HSEState = RCC_HSE_ON;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
  RCC_OscInitStruct.PLL.PLLM = 6;
  RCC_OscInitStruct.PLL.PLLN = 168;
  RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
  RCC_OscInitStruct.PLL.PLLQ = 4;
  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
  {
    Error_Handler();
  }

  /** Initializes the CPU, AHB and APB buses clocks
  */
  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
                              |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
  RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
  RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4;
  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2;

  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_5) != HAL_OK)
  {
    Error_Handler();
  }
}

/* USER CODE BEGIN 4 */

/* USER CODE END 4 */

/**
  * @brief  This function is executed in case of error occurrence.
  * @retval None
  */
void Error_Handler(void)
{
  /* USER CODE BEGIN Error_Handler_Debug */
  /* User can add his own implementation to report the HAL error return state */
  __disable_irq();
  while (1)
  {
  }
  /* USER CODE END Error_Handler_Debug */
}
#ifdef USE_FULL_ASSERT
/**
  * @brief  Reports the name of the source file and the source line number
  *         where the assert_param error has occurred.
  * @param  file: pointer to the source file name
  * @param  line: assert_param error line source number
  * @retval None
  */
void assert_failed(uint8_t *file, uint32_t line)
{
  /* USER CODE BEGIN 6 */
  /* User can add his own implementation to report the file name and line number,
     ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
  /* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */
